# usarmsg.py
# Contains regex patterns and string constructors for
# parsing and building messages

import re;
import socket;
import errno;
import string;

READ_LEN = 4096;
ENDL = "\r\n"

# INIT {ClassName robot_class} {Name robot_name} {Location x,y,z} {Rotation r,p,y}
initPattern = re.compile(r"""
    ^INIT\s*
    (?:\{\s*ClassName\s+(?P<class>\S+)\s*\}\s*)
    (?:\{\s*Name\s+(?P<name>\S+)\s*\}\s*)
    (?:\{\s*Location\s+
        (?P<x>[\d.]+)\s*,\s*
        (?P<y>[\d.]+)\s*,\s*
        (?P<z>[\d.]+)\s*\}\s*)?
    (?:\{\s*Rotation\s+
        (?P<roll>[\d.]+)\s*,\s*
        (?P<pitch>[\d.]+)\s*,\s*
        (?P<yaw>[\d.]+)\s*\}\s*)?$
    """, re.VERBOSE | re.IGNORECASE);

# DRIVE {Left float} {Right float} {Normalized bool} {Light bool} {Flip bool}
drivePattern1 = re.compile(r"""
    ^DRIVE\s*
    (?:\{\s*Left\s+(?P<left>\S+)\s*\}\s*)
    (?:\{\s*Right\s+(?P<right>\S+)\s*\}\s*)
    (?:\{\s*Normalized\s+(?P<normalized>\S+)\s*\}\s*)?
    (?:\{\s*Light\s+(?P<light>\S+)\s*\}\s*)?
    (?:\{\s*Flip\s+(?P<flip>\S+)\s*\}\s*)?$
    """, re.VERBOSE | re.IGNORECASE);

# DRIVE {Speed float} {FrontSteer float} {RearSteer float} {Normalized bool} {Light bool} {Flip bool}
drivePattern2 = re.compile(r"""
    ^DRIVE\s*
    (?:\{\s*Speed\s+(?P<speed>\S+)\s*\}\s*)
    (?:\{\s*FrontSteer\s+(?P<frontsteer>\S+)\s*\}\s*)
    (?:\{\s*RearSteer\s+(?P<rearsteer>\S+)\s*\}\s*)
    (?:\{\s*Normalized\s+(?P<normalized>\S+)\s*\}\s*)?
    (?:\{\s*Light\s+(?P<light>\S+)\s*\}\s*)?
    (?:\{\s*Flip\s+(?P<flip>\S+)\s*\}\s*)?$
    """, re.VERBOSE | re.IGNORECASE);

# DRIVE {Propeller float} {Rudder float} {SternPlane float} {Normalized bool} {Light bool}
drivePattern3 = re.compile(r"""
    ^DRIVE\s*
    (?:\{\s*Propeller\s+(?P<propeller>\S+)\s*\}\s*)
    (?:\{\s*Rudder\s+(?P<rudder>\S+)\s*\}\s*)
    (?:\{\s*SternPlane\s+(?P<sternplane>\S+)\s*\}\s*)
    (?:\{\s*Normalized\s+(?P<normalized>\S+)\s*\}\s*)?
    (?:\{\s*Light\s+(?P<light>\S+)\s*\}\s*)?$
    """, re.VERBOSE | re.IGNORECASE);

# DRIVE {AltitudeVelocity float} {LinearVelocity float} {LateralVelocity float} {RotationalVelocity float} {Normalized bool}
drivePattern4 = re.compile(r"""
    ^DRIVE\s*
    (?:\{\s*AltitudeVelocity\s+(?P<altitudevelocity>\S+)\s*\}\s*)
    (?:\{\s*LinearVelocity\s+(?P<linearvelocity>\S+)\s*\}\s*)
    (?:\{\s*LateralVelocity\s+(?P<lateralvelocity>\S+)\s*\}\s*)
    (?:\{\s*RotationalVelocity\s+(?P<rotationalvelocity>\S+)\s*\}\s*)
    (?:\{\s*Normalized\s+(?P<normalized>\S+)\s*\}\s*)?$
    """, re.VERBOSE | re.IGNORECASE);

inputPatterns = [drivePattern1, drivePattern2, drivePattern3, drivePattern4];

def parseLine(line):
    for pattern in inputPatterns:
        msg = pattern.match(line);
        if msg:
            return (msg, pattern);
    return None;

def readLine(sock, buffer):
    try:
        while True:
            data = sock.recv(READ_LEN);
            head, myendl, tail = data.partition("\r\n");    
            if myendl:
                return (buffer + head).rstrip(ENDL), tail;
            else:
                buffer += head;

    except socket.error, e:
        if (e[0] == errno.EAGAIN) or (e[0] == errno.EWOULDBLOCK):
            return None, buffer;
        else:
            raise;
